/// @file FlockManager.cpp
/// @brief the manager of the flock of boids, oversees their creation,
/// global movement, and drawing

#include "math.h"

#include "boost/foreach.hpp"

#include "ngl/NGLInit.h"

#include "FlockManager.h"
#include "Configuration.h"

/*--------------------------------------------------------------
Sampson T., Clamping Values
Available online at http://tomtech999.wordpress.com/2010/06/03/clamping-values/
Last Accessed : 26.03.2011
*/
#define min(a,b)                      (((a) < (b)) ? (a) : (b))
#define max(a,b)                      (((a) > (b)) ? (a) : (b))
#define clamp(value, lb, ub)          max( lb, min( ub, value ))
/*-------------------------------------------------------------*/


FlockManager::FlockManager(const bool _enable2DTopView)
{
    //read params from config
    Configuration::initialiseFlock(
                                    m_initialNumberOfBoids,
                                    m_normaliseRules,
                                    m_enableCohesion,
                                    m_enableSeparation,
                                    m_enableAlignment,
                                    m_cohesionWeight,
                                    m_separationWeight,
                                    m_alignmentWeight,
                                    m_separationDistance,
                                    m_neighbouringDistance,
                                    m_enableFOVNeighbouring,
                                    m_neighbouringFOVAngle,
                                    m_boidModel,
                                    m_tendPosition,
                                    m_tendTowardWeight,
                                    m_externalForce,
                                    m_externalForceWeight,
                                    m_enableTendToward,
                                    m_enableExternalForce,
                                    m_enableObstacleAvoidance,
                                    m_obstacleAvoidanceWeight,
                                    m_minSpeed,
                                    m_maxSpeed
                                    );


    //additional params initialisation
    m_enableAutoFlock = false;

    //2d top view
    m_enable2DTopView = _enable2DTopView;

    //trace boids and neighbours
    m_enableBoidNeighbourScreening = false;
    m_boidUnderNeighbourScreening = 0;

    //draw state
    m_wireframeBoids = false;

    //boid init params
    m_boidColor.set(1, 0, 0);
    m_enableRandomBoidColor = true;
    m_boidShader = "Light";
    m_drawBoidBoundingSphere = false;
    m_boidScaleFactor = 1;

    //create boids
    initialiseBoids();
}

FlockManager::~FlockManager()
{
    //clean up
    std::cout << "Flock Manager Cleanup" << std::endl;

    delete rng;

    //delete boids and clear the list
    deleteBoids();
    delete m_boidList;

    std::cout << "Flock Manager deleted\n" << std::endl;
}

void FlockManager::deleteBoids()
{
    //delete all boid objects in the list and clears up memory
    for (int i = 0; i < m_boidList->size(); i++) delete (*m_boidList)[i];
    m_boidList->clear();
}

void FlockManager::initialiseBoids()
{
    //init params
    m_nextBoidId = 0;

    m_boidList = new std::vector<Boid*>;

    //initialise the random number generator
    rng = ngl::Random::instance();

    for (int i = 0; i < m_initialNumberOfBoids; i++)
    {
        //create a new boid and add it to our list
        createBoid();
    }

    //initialise neighbours of boids
    refreshNeighbours();
}

void FlockManager::createBoid()
{
    ngl::Vector vel;
    ngl::Vector pos;
    ngl::Colour col;

    //create a position with random x, y and z in [-15, 15] (20 is the min size of the wall)
    int x = (int)rng->randomNumber(15);
    int y = (int)rng->randomNumber(15);
    int z = (int)rng->randomNumber(15);
    pos.set(x, y, z);

    //get initial vel [0, 1]
    vel = rng->getRandomVector();

    //set y components to 0 if 2d top view enabled
    if (m_enable2DTopView)
    {
        pos.m_y = 0;
        vel.m_y = 0;
    }

    //if random color enabled, get a random colour
    if (m_enableRandomBoidColor)
    {
        col = rng->getRandomColour();
        col.m_r = clamp(col.m_r, 0, 0.8);
        col.m_g = clamp(col.m_g, 0, 0.8);
        col.m_b = clamp(col.m_b, 0, 0.8);
    }
    else
    {
        //set color to prefixed color
        col.set(m_boidColor);
    }

    //add new boid to the list
    m_boidList->push_back(new Boid(m_nextBoidId++, col, pos, vel));
}

void FlockManager::addNewBoidToFlock()
{
    //create a new boid and add it to list
    createBoid();

    //refresh neighbours
    refreshNeighbours();

}

void FlockManager::reInitialise
                        (
                            const bool _enable2DTopView,
                            const int _initialNoOfBoids
                        )
{
    //save params values
    m_enable2DTopView = _enable2DTopView;
    m_initialNumberOfBoids = _initialNoOfBoids;

    //clear the list of all current boids
    deleteBoids();

    //recreate boids
    initialiseBoids();

    //trace boids and neighbours
    m_boidUnderNeighbourScreening = 0;
}

void FlockManager::reInitialise(const bool _enable2DTopView)
{
    //call overloaded method with current value of initial number of boids
    reInitialise(_enable2DTopView, m_initialNumberOfBoids);
}

void FlockManager::loadGeometry()
{
    ngl::VBOPrimitives* prim = ngl::VBOPrimitives::instance();

    //plane
    prim->createVBOQuadPlane("plane", 2, 1, 3, 3, ngl::Vector(0, 1, 0));

    //bounding sphere
    prim->createVBOSphere("boundingsphere", 1, 16);

    //bird parts
    prim->createVBOSphere("sphere", 1, 8);
    prim->createVBOQuadPlane("body", 2, 1, 3, 3, ngl::Vector(0, 1, 0));
    prim->createVBOQuadPlane("wings", 0.6, 2, 3, 3, ngl::Vector(0, 1, 0));
}

void FlockManager::draw
                    (
                        ngl::TransformStack _txStack,
                        ShaderManager* io_sman
                    )
{
    ngl::VBOPrimitives* prim=ngl::VBOPrimitives::instance();

    //loop through all boids
    BOOST_FOREACH(Boid* boid, *m_boidList)
    {
        //draw each boid, depending on the model chosen
        if (m_boidModel == "bird")
            drawComplexModel(boid, _txStack, io_sman);
        else
            drawPrimitiveModel(boid, _txStack, io_sman);

        //draw bounding sphere
        if (m_drawBoidBoundingSphere)
        {
            //draw bounding sphere
            //we dont use individual boid->getDrawBoundingSphere(), but global flock.m_drawBoidBoundingSphere

            glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
            _txStack.pushTransform();
            {
                //move the boid to its position
                _txStack.getCurrentTransform().setPosition(boid->getPosition());

                //set scale of boid = global scale factor
                _txStack.getCurrentTransform().setScale(m_boidScaleFactor, m_boidScaleFactor, m_boidScaleFactor);

                //update transform and color to shader
                io_sman->updateModelColor("Colour", _txStack.getCurrAndGlobal().getMatrix(), boid->getColour());

                //draw sphere
                prim->draw("boundingsphere");
            }
            _txStack.popTransform();
            glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        }
    }
}

void FlockManager::drawTendTowardTarget
                            (
                                ngl::TransformStack _txStack,
                                ShaderManager* io_sman,
                                const std::string _shader
                            )
{
    ngl::VBOPrimitives* prim=ngl::VBOPrimitives::instance();

    //draw the tend toward marker if enabled
    if (m_enableTendToward)
    {
        _txStack.pushTransform();
        {
            _txStack.getCurrentTransform().setPosition(m_tendPosition);

            _txStack.getCurrentTransform().setScale(2, 2, 2);

            //update transform and color to shader
            io_sman->updateModelColor(_shader, _txStack.getCurrAndGlobal().getMatrix(), ngl::Colour(1, 1, 0));

            prim->draw("cube");

        }
        _txStack.popTransform();
    }
}

void FlockManager::moveFlock
                        (
                            const Wall* _wall,
                            ObstacleManager* io_obstacleManager
                        )
{
    //loop through the boid list
    BOOST_FOREACH(Boid* boid, *m_boidList)
    {
        //determine next move for boid -> pass in all of the weights and thresholds
        boid->planNextMove
                (
                    m_normaliseRules,
                    m_separationDistance,
                    m_enableCohesion,
                    m_enableSeparation,
                    m_enableAlignment,
                    m_cohesionWeight,
                    m_separationWeight,
                    m_alignmentWeight,
                    m_enableObstacleAvoidance,
                    m_obstacleAvoidanceWeight,
                    m_enableTendToward,
                    m_tendPosition,
                    m_tendTowardWeight,
                    m_enableExternalForce,
                    m_externalForce,
                    m_externalForceWeight,
                    m_minSpeed,
                    m_maxSpeed,
                    m_boidScaleFactor,
                    io_obstacleManager,
                    m_enable2DTopView
                );

        //move boid freely without wall restriction
        boid->move();

        //keep boid within wall bounds
        keepBoidWithinBoundaries(boid, _wall);

        //update neighbours
        refreshNeighboursForABoid(boid);

    }
}

void FlockManager::keepBoidWithinBoundaries
                                    (
                                        Boid* io_boid,
                                        const Wall* _wall
                                    )
{
    //check is boids are withing wall bounds
    ngl::Vector deltaPosition = 0;
    if (_wall->isWithinBounds(io_boid, deltaPosition))
    {
        //is within bounds -> boid is safe -> do nothing
    }
    else
    {
        //implement wall limiting

        if (_wall->getHardWall())
        {
            //hard wall -> move back to within wall bounds
            io_boid->move(-deltaPosition);

            //then reverse velocity
            io_boid->reverseVelocity();
        }
        else
        {
            //soft wall -> allows boids to fly out but add a velocity to steer them back

            //calculate a tend towards origin velocity
            ngl::Vector tendTowardInside = io_boid->applyTendToward(0.1, true, ngl::Vector(0, 0, 0));

            //and determine a small increment to it -> controllable weight
            ngl::Vector weightedVector = Utilities::scaleVectorBy(tendTowardInside, _wall->getSoftLimitWeight());

            //adds the tend towards velocity to current velocity to steer boid inside gradually
            io_boid->addToVelocity(weightedVector);
        }
    }

}

void FlockManager::refreshNeighbours()
{
    //loop through list of boids
    BOOST_FOREACH(Boid* boid, *m_boidList)
    {
        //refresh neighbours of the boid in consideration
        refreshNeighboursForABoid(boid);
    }
}

void FlockManager::refreshNeighboursForABoid(Boid* io_centerBoid)
{
    //clear all past neighbours
    io_centerBoid->clearNeighbours();

    //process all boids around centerBoid and consider x number of boids in proximity
    BOOST_FOREACH(Boid* boid, *m_boidList)
    {
        //ensure we are not considering the centerBoid
        if (boid->getId() == io_centerBoid->getId()) continue;

        //PERFORM RULES TO DETERMINE NEIGHBOURS
        //DISTANCE <= PROXIMITY_THRESHOLD && ANGLE_EYE <= ANGLE_THRESHOLD
        bool okToAdd = true;

        //get orientation vector from center boid to test boid
        ngl::Vector vector = boid->getPosition() - io_centerBoid->getPosition();

        //get the distance between them
        float distance = vector.length();

        //disregard if it is too far away
        if (distance > m_neighbouringDistance) okToAdd = false;

        //evaluate fov calculations if enabled
        if (m_enableFOVNeighbouring)
        {
            //get the center boid's velocity
            ngl::Vector centerBoidVelocity = Utilities::normaliseVector(io_centerBoid->getVelocity());

            //normalise all vectors and get the dot (angle) between them
            vector = Utilities::normaliseVector(vector);
            float angleInBetween = acos(centerBoidVelocity.dot(vector)) * (180.0 / M_PI);

            //disregard if fov angle is too much -> boid is way out of focus for neighbouring
            if (angleInBetween > m_neighbouringFOVAngle) okToAdd = false;
        }

        //if ok to add as neighbour, add boid to centerBoid's neighbour list
        if (okToAdd) io_centerBoid->addNeighbour(boid);

    }
}

bool FlockManager::isANeighbour
                    (
                        const int _mainId,
                        const int _testId
                    )
{
    //get neighbours of mainId
    Boid* mainBoid = (*m_boidList)[_mainId];
    std::vector<Boid*>* neighbours = mainBoid->getNeighbourList();

    bool isNeighbour = false;
    Boid* b;
    for (int i = 0; i < neighbours->size(); i++)
    {
        b = (*neighbours)[i];
        if (b->getId() == _testId) isNeighbour = true;
    }

    return isNeighbour;
}

void FlockManager::drawComplexModel
                            (
                                const Boid* _boid,
                                ngl::TransformStack _txStack,
                                ShaderManager* io_sman
                            )
{
    ngl::VBOPrimitives* prim=ngl::VBOPrimitives::instance();

    //params used in positioning of body parts
    float distanceToWings = 0.2 * m_boidScaleFactor;
    float distanceToHead = 0.6 * m_boidScaleFactor;
    float scaleOfHead = 0.5 * m_boidScaleFactor;
    float distanceToEyes = 0.9 * m_boidScaleFactor;
    float scaleOfEyes = 0.2 * m_boidScaleFactor;

    float angleOfEyes = 45;

    float pitchz = _boid->getAngleOfPitch();
    float yawy = _boid->getAngleOfYaw();

    //calculate orientation based on the pitch and yaw of the boid
    float xdisp = cos(pitchz * (M_PI / 180.0)) * cos(yawy * (M_PI / 180.0));
    float ydisp = sin(pitchz * (M_PI / 180.0));
    float zdisp = cos(pitchz * (M_PI / 180.0)) * sin(yawy * (M_PI / 180.0));


    //calculate orientation of eye (given an extra angleOfEyes)
    float xdispEyes = cos((pitchz + angleOfEyes) * (M_PI / 180.0)) * cos(yawy * (M_PI / 180.0));
    float ydispEyes = sin((pitchz + angleOfEyes) * (M_PI / 180.0));
    float zdispEyes = cos((pitchz + angleOfEyes) * (M_PI / 180.0)) * sin(yawy * (M_PI / 180.0));

    //build orientation vectors
    ngl::Vector orientation(xdisp, ydisp, zdisp);
    ngl::Vector orientationEyes(xdispEyes, ydispEyes, zdispEyes);

    //center calculations around the boid's position
    ngl::Vector center(_boid->getPosition());

    if (m_wireframeBoids) glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

    //set color of boid
    setBoidColorToShader(_boid, io_sman);

    //draw body
    _txStack.pushTransform();
    {
        //go to center
        _txStack.getCurrentTransform().setPosition(center);

        //global boid scale
        _txStack.getCurrentTransform().setScale(m_boidScaleFactor, m_boidScaleFactor,  m_boidScaleFactor);

        //boid orientation
        _txStack.getCurrentTransform().setRotation(0, -_boid->getAngleOfYaw(), 0);
        _txStack.getCurrentTransform().addRotation(0, 0, _boid->getAngleOfPitch());

        //update shader with transform changes
        io_sman->updateModel(m_boidShader, _txStack.getCurrAndGlobal().getMatrix());

        //draw body
        prim->draw("body");
    }
    _txStack.popTransform();

    //draw wings
    _txStack.pushTransform();
    {
        //go to center of wings
        _txStack.getCurrentTransform().setPosition(center);
        _txStack.getCurrentTransform().addPosition(orientation * distanceToWings);

        //global boid scale
        _txStack.getCurrentTransform().setScale(m_boidScaleFactor, m_boidScaleFactor,  m_boidScaleFactor);

        //apply boid orientation
        _txStack.getCurrentTransform().setRotation(0, -_boid->getAngleOfYaw(), 0);
        _txStack.getCurrentTransform().addRotation(0, 0, _boid->getAngleOfPitch());

        //update shader with transform changes
        io_sman->updateModel(m_boidShader, _txStack.getCurrAndGlobal().getMatrix());

        //draw wings
        prim->draw("wings");
    }
    _txStack.popTransform();

    //draw head
    _txStack.pushTransform();
    {
        //go to center of head
        _txStack.getCurrentTransform().setPosition(center);
        _txStack.getCurrentTransform().addPosition(orientation * distanceToHead);

        //global scale * scale of head
        _txStack.getCurrentTransform().setScale(scaleOfHead, scaleOfHead, scaleOfHead);

        //update shader with transform changes
        io_sman->updateModel(m_boidShader, _txStack.getCurrAndGlobal().getMatrix());

        //draw head
        prim->draw("sphere");
    }
    _txStack.popTransform();

    //draw eye
    _txStack.pushTransform();
    {
        //go to center of eyes
        _txStack.getCurrentTransform().setPosition(center);
        _txStack.getCurrentTransform().addPosition(orientationEyes * distanceToEyes);

        //global scale * scale of eyes
        _txStack.getCurrentTransform().setScale(scaleOfEyes, scaleOfEyes, scaleOfEyes);

        //update shader with transform changes
        io_sman->updateModel(m_boidShader, _txStack.getCurrAndGlobal().getMatrix());

        //draw eyes
        prim->draw("sphere");
    }
    _txStack.popTransform();

    if (m_wireframeBoids) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

void FlockManager::drawPrimitiveModel
                                (
                                    const Boid* _boid,
                                    ngl::TransformStack _txStack,
                                    ShaderManager* io_sman
                                )
{
    ngl::VBOPrimitives* prim=ngl::VBOPrimitives::instance();

    if (m_wireframeBoids) glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    _txStack.pushTransform();
    {
        //move the boid to its position
        _txStack.getCurrentTransform().setPosition(_boid->getPosition());

        //set the scale of the boid = global scale factor
        _txStack.getCurrentTransform().setScale(m_boidScaleFactor, m_boidScaleFactor,  m_boidScaleFactor);

        //add in orientation (yaw/pitch) [negate angles => opengl camera orientation]
        _txStack.getCurrentTransform().setRotation(0, -_boid->getAngleOfYaw(), 0);
        _txStack.getCurrentTransform().addRotation(0, 0, _boid->getAngleOfPitch());

        //update shader with transform changes
        io_sman->updateModel(m_boidShader, _txStack.getCurrAndGlobal().getMatrix());

        //set boid colour
        setBoidColorToShader(_boid, io_sman);

        //draw model
        prim->draw(m_boidModel);
    }
    _txStack.popTransform();
    if (m_wireframeBoids) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

void FlockManager::setBoidColorToShader
                            (
                                const Boid* _boid,
                                ShaderManager* io_sman
                            )
{
    ngl::Colour finalColour(0, 0, 0);

    //different colours when we are viewing the neighbours of a particular boid
    if (!m_enableBoidNeighbourScreening)
    {
        //no neighbour viewing -> display normal boid
        finalColour.set(_boid->getColour().m_r, _boid->getColour().m_g, _boid->getColour().m_b);
    }
    else
    {
        //neighbour viewing enabled

        if (m_boidUnderNeighbourScreening == _boid->getId())
        {
            //this is the main boid which is being considered for neighbour viewing
            finalColour.set(1, 1, 1);
        }
        else
        {
            //not the main boid
            //lets check if this boid is atleast the neighbour of the main boid

            if (isANeighbour(m_boidUnderNeighbourScreening, _boid->getId()))
            {
                //is a neighbour
                finalColour.set(0, 0, 0);
            }
            else
            {
                //normal boid
                finalColour.set(_boid->getColour().m_r, _boid->getColour().m_g, _boid->getColour().m_b);
            }
        }
    }

    //update shader with colour information
    io_sman->updateColor(m_boidShader, finalColour);

}
